import cv2
import os
import numpy as np
import matplotlib.pyplot as plt

image_index = -1
maxIndex = 12
maxIndex2=1
image_index2=-1
image_size = None
board_size = (8, 11)
image_corners = []
all_corners = []
all_birdcorners=[]
bird_imgcorners=[]
#filePath = "D:\\temp\\ConsoleApplication1\\ConsoleApplication1\\ConsoleApplication1\\"
filePath="D:\\temp\\RobotProject\\through\\"
cell_size = (15.0, 15.0)
object_points = []
object_birdpoint= np.zeros((88,1,3))
cameraMatrix=[]
distCoeffs = np.zeros((1, 5), np.float32)
tvecsMat = []
rvecsMat = []
img=cv2.imread('00.jpg')#提取像素用
mat=[]#提取的像素点
filePath2="00c.jpg"
worldcoordinate=[[]]



def addInexToName(index, fileDirectory, tail):
    m_int2str = "{:02d}".format(index)
    name = filePath + fileDirectory + "\\" + m_int2str + tail
    return name



#D:\temp\through\pic2


def getImageCorners():
    global image_index, image_size, all_corners,imageInput
    print("开始提取角点......\n")
    while True:
        image_index += 1
        if image_index >= maxIndex:
            break

        imageInput = cv2.imread(addInexToName(image_index,"pic",".jpg"))
        if image_index == 0:
            image_size = (imageInput.shape[1], imageInput.shape[0])

        ret,image_corners= cv2.findChessboardCorners(imageInput, board_size, None)
        if ret==False:
            print("can not find chessboard corners  in   " + addInexToName(image_index, "pic", ".jpg") + "!\n")
            exit(1)
        else :
            print(image_corners)
            view_gray = cv2.cvtColor(imageInput, cv2.COLOR_BGR2GRAY)
            cv2.cornerSubPix(view_gray, image_corners, (11, 11), (-1, -1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1))
            all_corners.append(image_corners)
            cv2.drawChessboardCorners(imageInput, board_size, image_corners, False)
            cv2.imshow("Camera Calibration", imageInput)
            cv2.imwrite(addInexToName(image_index, "pic", "_a.jpg"), imageInput)
            cv2.waitKey(1)

    print("角点提取完成！\n")


#相机内外参标定
def cameraCalibrate():
    print("开始标定......")
    global cameraMatrix ,distCoeffs,rvecsMat,tvecsMat
    for t in range(image_index):
        position = []
        for i in range(board_size[1]):
            for j in range(board_size[0]):
                realPoint = np.zeros((3,), dtype=np.float32)
                realPoint[0] = i * cell_size[0]
                realPoint[1] = j * cell_size[1]
                realPoint[2] = 0
                position.append(realPoint)
        object_points.append(position)
    # calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flags=0)
    ret ,cameraMatrix, distCoeffs, rvecsMat, tvecsMat= cv2.calibrateCamera(np.array(object_points), all_corners, image_size,None,None)
    print("标定完成！")
    print(rvecsMat)
    print("开始保存标定结果...")
    print("开始保存标定结果...")
    rotation_matrix, _ = cv2.Rodrigues(rvecsMat[0])
    return rotation_matrix
   


#保存参数
def outputCalibrateResult():
    print("开始保存标定结果...")
    with open("result3.txt", 'w') as fout:
        fout.write("摄像头内参矩阵：\n")
        np.savetxt(fout, cameraMatrix, fmt='%f', delimiter=' ')
        fout.write("\n\n摄像头畸变系数：\n")
        np.savetxt(fout, distCoeffs, fmt='%f', delimiter=' ')
        fout.write("\n\n\n")
        for i in range(image_index):
            rotation_matrix, _ = cv2.Rodrigues(rvecsMat[i])
            fout.write(f"第 {i + 1} 幅图像的旋转矩阵：\n")
            fout.write(str(rotation_matrix) + "\n")
            fout.write(f"第 {i + 1} 幅图像的平移向量：\n")
            fout.write(str(tvecsMat[i]) + "\n\n")
    print("保存完成！")




#通过标定好的内参纠正图像
def undistortImages():
    print("开始矫正图像...")
    mapx = np.zeros(image_size, dtype=np.float32)
    mapy = np.zeros(image_size, dtype=np.float32)
    R = np.eye(3, 3, dtype=np.float32)

    for i in range(image_index):
        mapx, mapy=cv2.initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, cv2.CV_32FC1)
        imageInput = cv2.imread(addInexToName(i, "pic", ".jpg"))  # Make sure addInexToName is defined
        imageCorrect = imageInput.copy()
        imageCorrect = cv2.remap(imageInput, mapx, mapy, cv2.INTER_LINEAR)
        cv2.imwrite(addInexToName(i, "pic", "_b.jpg"), imageCorrect)  # Make sure addInexToName is defined
    print("矫正结束")




#转变成鸟瞰图
def reProject():
    print("开始生成鸟瞰图...")
    board_w = board_size[0]
    board_h = board_size[1]
    for i in range(image_index):
        h = np.zeros((3, 3), dtype=np.float32)
        objPts = []
        imgPts = []
        indexArray = [1, board_w - 1, (board_h - 1) * board_w, (board_h - 1) * board_w + board_w - 1]

        for j in range(4):
            objPts.append(((object_points[i][indexArray[j]][0])*2+50, (object_points[i][indexArray[j]][1])*2 +50))
            imgPts.append((all_corners[i][indexArray[j]]))



        h = cv2.getPerspectiveTransform(np.array(objPts, dtype=np.float32), np.array(imgPts, dtype=np.float32))
        imageInput = cv2.imread(addInexToName(i, "pic", ".jpg"))  # Make sure addInexToName is defined
        birdImage = imageInput.copy()
        birdImage = cv2.warpPerspective(imageInput, h, image_size, flags=cv2.INTER_LINEAR + cv2.WARP_INVERSE_MAP + cv2.WARP_FILL_OUTLIERS)
        cv2.imwrite(addInexToName(i, "pic", "_c.jpg"), birdImage)  # Make sure addInexToName is defined
    print("鸟瞰图生成结束")
    return h




#求解像素精度
def Accuracy(filepath):
    srcimg = cv2.imread(filepath)
    #cvtColor(srcimg, gray, CV_RGB2GRAY)
    gray = cv2.cvtColor(srcimg, cv2.COLOR_BGR2GRAY)
    found, corners = cv2.findChessboardCorners(gray, board_size, None)

    print("检测到的原始角点坐标")
    for i in range(len(corners)):
        print(f"第{i}个点（{corners[i][0][0]},{corners[i][0][1]})")

    if found:
        print("成功检测到角点")
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1)
        cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        cv2.drawChessboardCorners(srcimg, board_size, corners, found)
        dstimg = cv2.resize(srcimg, (1000, 800))
        cv2.imshow("亚像素角点图", dstimg)
        #cv2.waitKey(20)
        print("检测到的0.1亚像素斜的图像角点坐标")
        jingdux = 0
        totaljindu=0
        for i in range(len(corners)):
            if i == 0:#跳过第一个交点
                pass
            else:
                jingdux = np.sqrt((corners[i][0][0] - corners[i - 1][0][0]) ** 2 + (corners[i][0][1] - corners[i - 1][0][1]) ** 2)
                jingdux = 15.0 / jingdux
                totaljindu+=jingdux
            
            
            print(f"第{i}个点（{corners[i][0][0]},{corners[i][0][1]})")
            print(f"其像素精度为：{jingdux}/像素")
        totaljindu=totaljindu/(len(corners)-9)#对精度进行微调
        print(f"其综合像素精度为：{totaljindu}/像素")
        return totaljindu
        


#回调函数
num=-1#用作计数
def on_EVENT_LBUTTONDOWN(event, x, y, flags, param):
    
    global num

    if event == cv2.EVENT_LBUTTONDOWN:
        num+=1
        mat.append(x)
        mat.append(y)
        xy = "%d,%d" % (x, y)
        cv2.circle(img, (x, y), 1, (255, 255, 0), thickness = -1)
        cv2.putText(img, xy, (x, y), cv2.FONT_HERSHEY_PLAIN,
                    1.0, (255,255,0), thickness = 1)
        print(mat[0+(2*num)])
        cv2.imshow("image", img)
    


#标定像素点
def display(imgsource):
    cv2.namedWindow("image")
    cv2.setMouseCallback("image", on_EVENT_LBUTTONDOWN)
    while(1):
        cv2.imshow("image", imgsource)
        if cv2.waitKey(0)&0xFF==27:
            break
    cv2.destroyAllWindows()
    return mat
    



#求解距离函数,目前只支持两点
def measuredis(imgsource):
    #先求解图片像素精度
    accurracy=0
    pixel=[]
    pixeldis=0
    realdis=0
    accurracy=Accuracy(filePath2)
    pixel=display(imgsource)#获得像素
    pixeldis=np.sqrt((pixel[2]-pixel[0])**2+(pixel[3]-pixel[1])**2)
    realdis=pixeldis*accurracy
    print(f"该两点间的距离为：{realdis}mm")


#临时定义的四个点
point1 = (175,319)
point2 = (341,319)
point3 = (341,399)
point4 = (175,399)


def drawrectangle():
    image = img

    # 定义四个点坐标
    

    # 画矩形框
    cv2.line(image, point1, point2, (255, 0, 0), 2)
    cv2.line(image, point2, point3, (255, 0, 0), 2)
    cv2.line(image, point3, point4, (255, 0, 0), 2)
    cv2.line(image, point4, point1, (255, 0, 0), 2)

    # 显示画好矩形框的图像
    cv2.imshow("Rectangle", image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()


def solve():
    x=(point2[0]-point1[0])*Accuracy(filePath2)
    y=(point3[1]-point2[1])*Accuracy(filePath2)
   
    word_point=np.array([
    [[0,0,0],
    [x,0,0],
    [x,y,0],
    [0,y,0]
    ]], dtype=np.float32)


    pixpoint=np.array([  
    [[175,319],
    [341,319],
    [341,399],
    [175,399]
    ]], dtype=np.float32)

    cameraMatrix=np.array(
    [[473.9139868712908, 0, 321.7644494481638],
    [0, 473.6163661302777, 233.7803126852079],
    [0, 0, 1]
    ],dtype=np.double)


    distCoeffs=np.array([
        [0.044242 ,-0.0387607, -0.0024813 ,-0.0019008,
    0.01407]],dtype=np.double)
   

    imageInput=cv2.imread(filePath2)
    image_size = (imageInput.shape[1], imageInput.shape[0])
    #ret ,cameraMatrix, distCoeffs, rvecsMat, tvecsMat= cv2.calibrateCamera((word_point),pixpoint, image_size,None,None)
    success, rotationVector, translationVector = cv2.solvePnP(word_point, pixpoint, np.array(cameraMatrix), distCoeffs,flags=0)
    rotation_matrix, _ = cv2.Rodrigues(rotationVector)
    print(rotation_matrix)
    return rotation_matrix




import math
def value():
    rotation_matrix=solve()

    # 假设 rotation_matrix 是您的旋转矩阵
    # 将旋转矩阵转换为欧拉角
    r = math.atan2(rotation_matrix[2, 1], rotation_matrix[2, 2])
    p = math.atan2(-rotation_matrix[2, 0], math.sqrt(rotation_matrix[2, 1] ** 2 + rotation_matrix[2, 2] ** 2))
    y = math.atan2(rotation_matrix[1, 0], rotation_matrix[0, 0])

    # 将弧度转换为角度
    r_angle = math.degrees(r)
    p_angle = math.degrees(p)
    y_angle = math.degrees(y)

    print("Roll (绕X轴旋转角度)：", r_angle)
    print("Pitch (绕Y轴旋转角度)：", p_angle)
    print("Yaw (绕Z轴旋转角度)：", y_angle)






def getbirdcorrners(filepath):
    global image_index2 #, image_size#,imageInput#, all_corners
    print("开始提取角点......\n")
    while True:
        image_index2 += 1
        if image_index2 >= maxIndex2:
            image_index2=-1
            break
        all_birdcorners = []
        imageInput = cv2.imread(filepath)
        if image_index == 0:
            image_size = (imageInput.shape[1], imageInput.shape[0])

        ret,image_corners= cv2.findChessboardCorners(imageInput, board_size, None)
        if ret==False:
            print("can not find chessboard corners  in   " + addInexToName(image_index, "picbird", ".jpg") + "!\n")
            exit(1)
        else :
            view_gray = cv2.cvtColor(imageInput, cv2.COLOR_BGR2GRAY)
            cv2.cornerSubPix(view_gray, image_corners, (11, 11), (-1, -1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1))
            all_birdcorners.append(image_corners)
            cv2.drawChessboardCorners(imageInput, board_size, image_corners, False)
            cv2.imshow("Camera Calibration", imageInput)
            cv2.imwrite(addInexToName(image_index, "picbird", "_a.jpg"), imageInput)
            cv2.waitKey(1) 
    print("角点提取完成！\n")
    print(all_birdcorners)
    return all_birdcorners





def remap(accuracyfilepath,corners):
    #global birdcorners
    x_values = np.empty(88)
    y_values = np.empty(88)
    accuracy=Accuracy(accuracyfilepath)
    #birdcorners=corners
    birdcorners=getbirdcorrners(accuracyfilepath)#单独运行时启用
   
    
    
    
    for i in range(len(birdcorners)):
        for j in range(len(birdcorners[i])):
            realpoint=np.zeros((3),dtype=np.float32)
            if j==0 :#以左下角第一个点为0点
                object_birdpoint[j][0][0]=0
                object_birdpoint[j][0][1]=0
                object_birdpoint[j][0][2]=0
            else:
                if(j>7):
                    base=j-7-1
                    object_birdpoint[j][0][0]=((accuracy*(birdcorners[i][j][0][0]-birdcorners[i][base][0][0])))+object_birdpoint[base][0][0]#相对于原点的x距离
                    object_birdpoint[j][0][1]=accuracy*(birdcorners[i][j][0][1]-birdcorners[i][base][0][1])+object_birdpoint[base][0][1]#相对于原点的y距离
                    object_birdpoint[j][0][2]=0
                else:
                    object_birdpoint[j][0][0]=((accuracy*(birdcorners[i][j][0][0]-birdcorners[i][j-1][0][0])))+object_birdpoint[j-1][0][0]#相对于原点的x距离
                    object_birdpoint[j][0][1]=(-accuracy*(birdcorners[i][j][0][1]-birdcorners[i][j-1][0][1])+object_birdpoint[j-1][0][1])#相对于原点的y距离
                    object_birdpoint[j][0][2]=0
            # object_birdpoint2.append(realpoint)#当有多张图片采用此方法
   
    print(object_birdpoint)#打印真实坐标系
    for i in range(len(object_birdpoint)):
         print(object_birdpoint[i][0][0])
         x_values[i]=object_birdpoint[i][0][0]
         y_values[i]=object_birdpoint[i][0][1]
    plt.scatter(x_values, y_values)
    plt.title('Connecting Points with Lines')
    plt.xlabel('X-axis')
    plt.ylabel('Y-axis')
    # 显示图形
    plt.show()         
          

          
#对有俯仰角的图片进行纠偏         
def rectifypix():
    corect_corners=getbirdcorrners("D:\\temp\\RobotProject\\through\pic\\02_c.jpg")
    Vdis=[]
    wrong_corners=getbirdcorrners("D:\\temp\\RobotProject\\through\pic\\02.jpg")

    print("正确像素坐标：",corect_corners)
    print("修改前的像素坐标：",wrong_corners)
    deviation=np.empty((88),dtype=np.float32)#偏差数组

     #定义左下角的点为原点
    # for i in range(len(corect_corners)):
    #     for j in range(len(corect_corners[i])):
    #         if j==0:#以左下角第一个点为0点
    #             baseX=corect_corners[i][j][0][0]
    #             baseY=corect_corners[i][j][0][1]
    #             deviation[0]=0
    #         else:
    #             deviation[j]=np.sqrt((corect_corners[i][j][0][0]-baseX)**2+(corect_corners[i][j][0][1]-baseY)**2)#到原点的距离，在像素坐标系下
    # print("下面的数据是误差：",deviation)
    # for i in range(len(wrong_corners)):
    #     x=0
    #     y=0
    #     for j in range(len(wrong_corners[i-1])):
    #         wrongbaseX=wrong_corners[i][7][0][0]
    #         wrongbaseY=wrong_corners[i][7][0][1]
            
    #         if(j==0):
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(8>j>0):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
               
    #         if(j==8):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(16>j>8):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
                
    #         if(j==16):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(24>j>16):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
              
    #         if(j==24):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(32>j>24):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
                
    #         if(j==32):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(40>j>32):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
               
    #         if(j==40):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(48>j>40):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
              
    #         if(j==48):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(56>j>48):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
              
    #         if(j==56):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(64>j>56):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
              
    #         if(j==64):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(72>j>64):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
                
    #         if(j==72):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(80>j>72):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
               
    #         if(j==80):
    #             x=0
    #             y=j+7
    #             extension_length=deviation[j+7]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
    #         if(j>80):
    #             x+=1
    #             extension_length=deviation[y-x]-np.sqrt((wrong_corners[i][j][0][0]-wrongbaseX)**2+(wrong_corners[i][j][0][1]-wrongbaseY)**2)
    #             Vdis=extend_line(wrong_corners[i][87][0][0],wrong_corners[i][87][0][1],wrong_corners[i][j][0][0],wrong_corners[i][j][0][1],extension_length)
    #             wrong_corners[i][j][0][0]+=Vdis[0]
    #             wrong_corners[i][j][0][1]+=Vdis[1]
               
    print("真实坐标系：",wrong_corners)#打印真实坐标系
    x_values = np.empty(88)
    y_values = np.empty(88)
    for i in range(len(wrong_corners[0])):
     #print(wrong_corners[i][0][0])
     x_values[i]=wrong_corners[0][i][0][0]
     y_values[i]=-wrong_corners[0][i][0][1]
    plt.scatter(x_values, y_values)
    plt.title('Connecting Points with Lines')
    plt.xlabel('X-axis')
    plt.ylabel('Y-axis')
    # 显示图形
    plt.show()     




def extend_line(x1, y1, x2, y2, extension_length):
    # 计算原始线段的斜率
    slope = (y2 - y1) / (x2 - x1)
    newxy=np.array([0,0],dtype=float)
    # 计算新的延长后的线段的终点坐标
    if(slope<0):
        new_x = - extension_length / math.sqrt(1 + slope ** 2)
        new_y =  -slope * (new_x)
    else:
        new_x =  extension_length / math.sqrt(1 + slope ** 2)
        new_y =  -slope * (new_x)
    newxy[0]=new_x
    newxy[1]=new_y
    print("这是需要增加的xy:",newxy)
    return newxy


def transfomcamera():
    deepZ=4
    corect_corners=getbirdcorrners("D:\\temp\\RobotProject\\through\pic\\02_c.jpg")
    cameraxis=np.empty((88,3),dtype=float)
    corners=np.empty((3,1),dtype=float)
    tempcameraxis=[]
    for i in range(len(corect_corners)):
        for j in range(len(corect_corners[i])):
            corners[0][0]=corect_corners[i][j][0][0]
            corners[1][0]=corect_corners[i][j][0][1]
            corners[2][0]=1
            tempcameraxis=np.transpose(np.linalg.inv(cameraMatrix)*deepZ @ corners)
            cameraxis[j]=tempcameraxis
    print(cameraxis)








if __name__ == "__main__":
    getImageCorners()
    cameraCalibrate()
    outputCalibrateResult()
    undistortImages()
    reProject()
    # Accuracy(filePath2)
    #measuredis(img)
    #display(img)
    #drawrectangle()
    #solve()
    #value()
    #getbirdcorrners()
    #remap("D:\\temp\\RobotProject\\through\pic\\02_c.jpg",1)
    #rectifypix()
    transfomcamera()
